#include "StateMachine.h"
#include "Ttask.h"
#include "cmsis_os.h"
extern StateMachine sm;
// Implementation of State methods
void State::onEnter() {
    // Default implementation does nothing
}

void State::onExit() {
    // Default implementation does nothing
}

State* State::handleEvent(Event event, int param) {
    // Default implementation ignores events and stays in the same state
    return this;
}
// Implementation of IDLE state
void IDLE::onEnter() {
    log_1->printf("Entering IDLE state");
    if (LimitLayerObj.LimitSwitchObj[Lim_6].TriggerState!=Trigger)
    {
        MotorLayerobj.BaseTask_RPS_Control(2,5);
				log_1->printf("BaseTask_RPS_Control");
        task.start(10,[=]() {
          if (LimitLayerObj.LimitSwitchObj[Lim_6].TriggerState==Trigger){
            MotorLayerobj.BaseTask_RPS_Control(2,0);
            return sm.enqueueEvent(Event::RUN, 0);
          }
        },TTASK::TimerType::PERIODIC);
    }else{
			return sm.enqueueEvent(Event::RUN, 0);
		}
    

    

}

void IDLE::onExit() {
    log_1->printf("Exiting IDLE state");
}

State* IDLE::handleEvent(Event event, int param) {
    switch (event) {
        case Event::RUN:
            return new TRANSFERRING(TRANSFER::NONE_);
        case Event::PULLING:
            return new PULLING(0);
        default:
            break;
    }
    return this;
}

// Implementation of CONFIRMING state
void CONFIRMING::onEnter() {
    log_1->printf("Entering CONFIRMING state");
    return sm.enqueueEvent(Event::LIMITATION_1, 0);
}

void CONFIRMING::onExit() {
    log_1->printf("Exiting CONFIRMING state");
}

State* CONFIRMING::handleEvent(Event event, int param) {
    switch (event) {
        case Event::CONFIRM_OK:
            return new TRANSFERRING(param_);
        case Event::LIMITATION_1:
        case Event::LIMITATION_2:
            return new IDLE();
        default:
            break;
    }
    return this;
}

uint8_t flag = 255;
uint8_t first_flag = 0;
bool pushRodCompleted = false;
bool needRestartConveyor0 = false;
bool needRestartConveyor1 = false;
extern int targetPosition;
// Implementation of TRANSFERRING state
void TRANSFERRING::onEnter() {
    log_1->printf("Entering TRANSFERRING state");
    MotorLayerobj.BaseTask_RPS_Control(0, -10);
    MotorLayerobj.BaseTask_RPS_Control(1, -15);

	
                    
		
											
											
//		task[7].start(100, [=]() {
//                       if (LimitLayerObj.LimitSwitchObj[4].TriggerState == Trigger)
//											 {
//											MotorLayerobj.BaseTask_RelativeAngleFigure(3, 20, -1*90*6.725);   
//											   task[8].start(2000, [=]() {   
//												 MotorLayerobj.BaseTask_RelativeAngleFigure(3, 20, 10*360*6.725);
//													 if(LimitLayerObj.LimitSwitchObj[4].TriggerState == Trigger){
//				
//					
//			
//				              MotorLayerobj.BaseTask_RPS_Control(3, 0);
//												task[8].stop();	 
//													 }
//													  },TTASK::TimerType::PERIODIC);		
//												 task[7].stop();
//											 }
//											 

//                    },TTASK::TimerType::SINGLE_SHOT);									 
     
    MotorLayerobj.BaseTask_RelativeAngleFigure(3, 20, 10*360*6.725);
    STARTtask[0].start(10, [&]() {
			if(LimitLayerObj.LimitSwitchObj[4].TriggerState == Trigger){
				
				MotorLayerobj.BaseTask_RPS_Control(3, 0);
				STARTtask[0].stop();
			}
		},TTASK::TimerType::PERIODIC);  


    task[0].start(10, [&]() {
        if (LimitLayerObj.LimitSwitchObj[2].TriggerState == Trigger) {
            task[2].start(1000, [=]() {
                MotorLayerobj.BaseTask_RPS_Control(0, 0); 
            },TTASK::TimerType::SINGLE_SHOT);
            task[0].stop();
        }
    },TTASK::TimerType::PERIODIC);
    task[1].start(10, [&]() {
        if (LimitLayerObj.LimitSwitchObj[3].TriggerState == Trigger) {
            task[3].start(1000, [=]() {
                MotorLayerobj.BaseTask_RPS_Control(1, 0); 
                task[4].start(2000, [=]() {
                    task[5].start(500, [=]() {
                        switch (SEND_MEM_RX->Rubbish) {
                        case Hazardous:
                            targetPosition = 1;
                            break;
                        case Recyclable:
                            targetPosition = 2;
                            break;
                        case kitchen:
                            targetPosition = 3;
                            break;
                        case Other:
                            targetPosition = 4;
                            break;
                        default:
                            targetPosition = 0;
                            log_1->printf(LOG_LEVEL_ERROR, "RS485", "no targetPosition!");
												if (LimitLayerObj.LimitSwitchObj[3].TriggerState == Trigger)
											 {
											 MotorLayerobj.BaseTask_RPS_Control(1, -15);
											break;
											 }
                            
                        }
                        if (targetPosition)
                        {
                            return sm.enqueueEvent(Event::DETECED, 0);
                        }
                    },TTASK::TimerType::PERIODIC);
                },TTASK::TimerType::SINGLE_SHOT);
            },TTASK::TimerType::SINGLE_SHOT);  
            task[1].stop();
        }
   },TTASK::TimerType::PERIODIC);
		
	    	 task[6].start(1500, [=]() {
                       if (LimitLayerObj.LimitSwitchObj[2].TriggerState == Trigger)
											 {
											 MotorLayerobj.BaseTask_RPS_Control(0, -10);
											 
											 }
											 
//        task[6].stop();
                    },TTASK::TimerType::PERIODIC);

	 
}

void TRANSFERRING::onExit() {
    log_1->printf("Exiting TRANSFERRING state");
}

State* TRANSFERRING::handleEvent(Event event, int param) {
    switch (event) {
        case Event::DETECED:
            // Perform transfer operation based on param_
            return new ROTATE(param);
        default:
            break;
    }
    return this;
}


void PULLING::onEnter() {
    log_1->printf(LOG_LEVEL_INFO, "PULLING::onEnter", "Entering PULLING");
    if (LimitLayerObj.LimitSwitchObj[Lim_5].TriggerState != Trigger) {
        MotorLayerobj.BaseTask_RPS_Control(2,-5);
        log_1->printf("BaseTask_RPS_Control(2,-5)");
        task.start(10,[=]() {
            if (LimitLayerObj.LimitSwitchObj[Lim_5].TriggerState==Trigger){
                MotorLayerobj.BaseTask_RPS_Control(2,0);
                task.stop();
                return sm.enqueueEvent(Event::PULLING, 0);
            }
        },TTASK::TimerType::PERIODIC);
    } else {
        return sm.enqueueEvent(Event::PULLING, 0);
    }
    //        return sm.enqueueEvent(Event::PULLING, param_); // ´¥·¢×´Ì¬×ª»»µ½
}

void PULLING::onExit() {
	log_1->printf(LOG_LEVEL_INFO, "PULLING::onExit", "Exiting PULLING");
	SEND_MEM_TX->Inf=EXECUTION_OK;
	// memset(&USARTbuftx,0,sizeof(USARTbuftx));
	osDelay(2000);
	SEND_MEM_TX->Inf=EXECUTION_BUSY;
}

State *PULLING::handleEvent(Event event, int param) {
	switch (event) {
		// case Event::DETECED:
		//   log_1->printf(LOG_LEVEL_INFO, "PULLING::handleEvent",
		//                 "PULLING: Handling DETECED, transition to IDLE");
		// return new IDLE();
		// break;
	case Event::PULLING:
		log_1->printf(LOG_LEVEL_INFO, "PULLING::handleEvent","PULLING: Handling ROTATE, transition to ROTATE");
		return new IDLE();
		break;
	default:
		// std::cout << "PULLING: Unknown event, staying in PULLING" << std::endl;
		break;
	}
	return this;
}

// ROTATE
  
int angle =0;

//int currentPosition = 1;
void ROTATE::onEnter() {
    log_1->printf(LOG_LEVEL_INFO, "ROTATE::onEnter", "Entering ROTATE");
    log_1->printf(LOG_LEVEL_DEBUG, "ROTATE::onEnter", "targetPosition=%d",targetPosition);
    MotorLayerobj.BaseTask_RelativeAngleFigure(3, 6, angle);
    switch (targetPosition-1) {
    case 0:
        angle = 0;
		    MotorLayerobj.BaseTask_RelativeAngleFigure(3, 6, angle);
		    osDelay(2000);
        break;
    case 1:
        angle = 1* 90*6.725;
		    MotorLayerobj.BaseTask_RelativeAngleFigure(3, 6, angle);
		    osDelay(2000);
        break;
		case 2:
			  angle = 2* 90*6.725;
		    MotorLayerobj.BaseTask_RelativeAngleFigure(3, 6, angle);
		    osDelay(3000);

        break;
		case 3:
			  angle = -1* 90*6.725;
		    MotorLayerobj.BaseTask_RelativeAngleFigure(3, 6, angle);
        osDelay(2000);
        break;
		default:
        log_1->printf(LOG_LEVEL_INFO, "ROTATE",
                    "ROTATE: Unknown event, staying in ROTATE");
        break;
    }
//    currentPosition = targetPosition; 
    sm.enqueueEvent(Event::RUN, param_);

}
void ROTATE::onExit() {
    log_1->printf(LOG_LEVEL_INFO, "ROTATE::onExit", "Exiting ROTATE");
}


State *ROTATE::handleEvent(Event event, int param) {
    switch (event) {
    case Event::RUN:
        log_1->printf(LOG_LEVEL_INFO, "ROTATE::handleEvent",
                    "ROTATE: Handling DETECED, transition to PULLING");
        return new PULLING(param);
        break;
    default:
        log_1->printf(LOG_LEVEL_INFO, "ROTATE",
                    "ROTATE: Unknown event, staying in ROTATE");
        break;
    }
    return this;
}


// Implementation of StateMachine methods
void StateMachine::enqueueEvent(Event event, int param) {
    eventQueue.push({event, param});
}

void StateMachine::processEvents() {
    if (!eventQueue.empty()) { // 仅检查一次而非循环处理
        auto &front = eventQueue.front();
        Event event = front.first;
        int param = front.second;
        
        // 调试信息保留
        int count = eventQueue.size();
        log_1->printf("count1=%d", count);
        eventQueue.pop();
        count = eventQueue.size();
        log_1->printf("count2=%d", count);
        
        sm.processEvent(event, param); // 仅处理一个事件
    }
}
void StateMachine::start(State* initialState) {
    if (currentState != nullptr) {
        currentState->onExit();
        delete currentState;
    }
    currentState = initialState;
    if (currentState) {
        currentState->onEnter();
    }
}

void StateMachine::processEvent(Event event, int param) {
    if (!currentState) return;
    State* nextState = currentState->handleEvent(event, param);
    if (nextState != currentState && nextState != nullptr) {
        start(nextState);
    }
}